//
// Created by jwhww on 25-11-2.
//

#include "AllHeadFile.h"

//------------------------------------------------------------------------------------------------------
// 函数简介         M3508供弹轮盘堵转检测
// 使用示例         JJ_motor_roulette_blocking_meter(Motor_Info_M3508, MOTOR_1);
// 参数介绍         current                                 当前电流（单位：毫安），-16384~16383
// 参数介绍         speed                                   当前速度（单位：RPM），-3200~3200
// 返回参数         result                                  计算结果
// 备注信息         只测试过英雄供弹，步兵未知；使用示例在后面
//------------------------------------------------------------------------------------------------------
void JJ_motor_roulette_blocking_meter(MotorInfo *motor_info, const MotorID motor_id) {
    //1-首次判断堵转
    static int32_t Roulette_angle = 0; //轮盘位置
    static float diff = 0;              //反转轮盘转过位置
    if (abs(motor_info[motor_id].torque_current) > (int)BLOCKING_GATE_CURRENT
    && abs(motor_info[motor_id].motor_speed) < (int)BLOCKING_GATE_SPEED
    && motor_info[motor_id].blocking_status == NOTBLOCKING) {
        motor_info[motor_id].blocking_status = BLOCKING_DOUBTFUL;
        motor_info[motor_id].blocking_data.time_when_blocking = HAL_GetTick();
    }
    //2- BLOCKING_TIM 时间内跳过所有判断
    if (motor_info[motor_id].blocking_status == BLOCKING_DOUBTFUL) {
        if (HAL_GetTick() - motor_info[motor_id].blocking_data.time_when_blocking < BLOCKING_TIM) {
            return;
        }
        //2- 二重判
        if (abs(motor_info[motor_id].torque_current) > (int)BLOCKING_GATE_CURRENT
        && abs(motor_info[motor_id].motor_speed) < (int)BLOCKING_GATE_SPEED) {
            //堵转时状态暂存
            motor_info[motor_id].blocking_data.motor_angle = motor_info[motor_id].motor_angle;
            motor_info[motor_id].blocking_data.torque_current = motor_info[motor_id].torque_current;
            motor_info[motor_id].blocking_data.motor_speed = motor_info[motor_id].motor_speed;
            Roulette_angle = (int32_t)((float)motor_info[motor_id].blocking_data.motor_angle * 19.203f);  //转盘角度，157310 = 8192*19.203f
            if (motor_info[motor_id].blocking_data.torque_current > 0) {
                motor_info[motor_id].blocking_status = BLOCKING_COMFIRM_POSITIVE;
            }
            else {
                motor_info[motor_id].blocking_status = BLOCKING_COMFIRM_NEGATIVE;
            }
            return;
        }
        motor_info[motor_id].blocking_status = NOTBLOCKING;
    }
    //3-反转
    if (motor_info[motor_id].blocking_status == BLOCKING_COMFIRM_POSITIVE) {  //0.001f是tim6的中断时间，因为这个函数在tim6里面运行
        motor_info[motor_id].blocking_data.data_back_reverse = JJ_motor_speed_closeCircle(-(SPEED_MAX_M3508), motor_info, motor_id);
        diff += CALCULATE_ROULETTE_DIFF(motor_info[motor_id].motor_speed);  //累计轮盘编码器
        if (diff > ROULETTE_SCALE/2) diff -= ROULETTE_SCALE; //最短路径归中，3508的r/m换算为r/s，再换算为r/ms，再除以减速比换算为轮盘r/ms，再乘ROULETTE_SCALE换算为轮盘相对编码器值。
        if (diff < -ROULETTE_SCALE/2) diff += ROULETTE_SCALE;
        if (fabs((double)diff) > ROULETTE_BACK_DISTANCE){//电流和速度同方向，所以这里速度反向就相当于电流反向
            motor_info[motor_id].blocking_status = BLOCKING_BACK_FINISH;
            diff = 0;
            Roulette_angle = 0;
            pid_refresh_output(&motor_info[motor_id].Struct_Pid.SPEED);   //清除速度增量式累计输出
            motor_info[motor_id].blocking_data.motor_angle = 0;
            motor_info[motor_id].blocking_data.torque_current = 0;
            motor_info[motor_id].blocking_data.motor_speed = 0;
            return;
        }
    }
    if (motor_info[motor_id].blocking_status == BLOCKING_COMFIRM_NEGATIVE) {
        motor_info[motor_id].blocking_data.data_back_reverse = JJ_motor_speed_closeCircle((SPEED_MAX_M3508), motor_info, motor_id);
        diff += CALCULATE_ROULETTE_DIFF(motor_info[motor_id].motor_speed);
        if (diff > ROULETTE_SCALE/2) diff -= ROULETTE_SCALE;
        if (diff < -ROULETTE_SCALE/2) diff += ROULETTE_SCALE;
        if (fabs((double)diff) > ROULETTE_BACK_DISTANCE){//电流和速度同方向，所以这里速度反向就相当于电流反向
            motor_info[motor_id].blocking_status = BLOCKING_BACK_FINISH;
            diff = 0;
            Roulette_angle = 0;
            pid_refresh_output(&motor_info[motor_id].Struct_Pid.SPEED);
            motor_info[motor_id].blocking_data.motor_angle = 0;
            motor_info[motor_id].blocking_data.torque_current = 0;
            motor_info[motor_id].blocking_data.motor_speed = 0;
            return;
        }
    }
    //4- 电机回0
    if (motor_info[motor_id].blocking_status == BLOCKING_BACK_FINISH) {
        motor_info[motor_id].blocking_data.data_back_reverse = JJ_motor_speed_closeCircle(0, motor_info, motor_id);
        if (motor_info[motor_id].motor_speed == 0) {
            motor_info[motor_id].blocking_status = NOTBLOCKING;
        }
    }
}
/** 堵转检测使用示例，需配合状态机！！！！！！！！！！！！！！！！！！！！！！（其实只有jwh能看懂——jwh）
 *
*    //装单轮盘电机(设置最大力矩为最大可达力矩的0.5倍）
        //单速度环
        //堵转检测
        JJ_motor_roulette_blocking_meter(Motor_Info_M3508, MOTOR_1);    //使用SPEED环参数（10.f, 0.5f, 0.f）
        if (Motor_Info_M3508[MOTOR_1].blocking_status != NOTBLOCKING && Motor_Info_M3508[MOTOR_1].blocking_status != BLOCKING_DOUBTFUL) {
            //堵转状态
            readyToload_M3508[0] = Motor_Info_M3508[MOTOR_1].blocking_data.data_back_reverse;
        } else {
            //正常状态
            temp_data_M3508[0] = -((float)dbus_ctrl_data.channel.CH[3] - 1024.f) * (SPEED_MAX_M3508/660.f);  // 0.71 = 469 / (1684 - 1024)
            readyToload_M3508[0] = JJ_motor_speed_closeCircle((int16_t)temp_data_M3508[0], Motor_Info_M3508, MOTOR_1);
        }
 */